#include<ros/ros.h> 
#include<iostream> 
#include<cv_bridge/cv_bridge.h> 
#include<sensor_msgs/image_encodings.h> 
#include<image_transport/image_transport.h> 
#include<opencv2/core/core.hpp>
#include<opencv2/highgui/highgui.hpp>
#include<opencv2/imgproc/imgproc.hpp>
#include "std_srvs/Empty.h"
#include "opencv2/opencv.hpp"
#include <stdio.h>
#include <stdlib.h>
#include <cmath>
#include "opencv_lidar.h"
#include "geometry_msgs/Point.h"
#include "sensor_msgs/LaserScan.h"
#include <std_msgs/Float64.h>

// #ifndef _countof
// #define _countof(_Array) (int)(sizeof(_Array) / sizeof(_Array[0]))
// #endif

// #define DEG2RAD(x) ((x)*M_PI/180.)
// #define RAD2DEG(x) ((x)*180./M_PI)
// #define PI 3.141592653

using namespace cv;
using namespace std;

vector<scanDot> scan_data;
int CALPOINT;

void scanCallback(const sensor_msgs::LaserScan::ConstPtr& scan)
{
    scan_data.clear();
    // int lidarcount = int(scan->scan_time / scan->time_increment); //总点数
	// ROS_INFO("scanCallback   lidarcount: [%d]", lidarcount);  //1439或1438
	int lidarcount=1441;
	scanDot dot;
    for(int i = 0; i < lidarcount; i++)
	{
        if(scan->ranges[i] < 3  && scan->ranges[i] > 0.2)//忽略太远或太近的
        {
			dot.angle = (scan->angle_min + scan->angle_increment * i)*57.2958; //弧度制转角度制
			if(dot.angle > 320 || dot.angle < 40)
			{
				dot.dist = scan->ranges[i]*1000; //毫米制
				scan_data.push_back(dot);
				// ROS_INFO(": [%f, %f]", dot.angle, scan->ranges[i]);
			}
        }            
    }
}


void DistanceCallback(const std_msgs::Float64& distance)
{


	if(distance.data> 31.5 && distance.data < 33)//rec1
	{
		endSignal = 2; 
		CALPOINT=25;
	}
	else if(distance.data> 66.5 && distance.data < 70 )//rec2
	{
		endSignal = 2;
		CALPOINT=36; 
	}
	else if(distance.data> 35)//round2
	{
		CALPOINT=36;
		endSignal = 10;
	}
	else
	{
		CALPOINT=21;
		endSignal = 0;

	}

	if(distance.data> 74)
	{
		endSignal = 1; 
	}
}

int main( int argc, char **argv )
{
	ros::init( argc, argv, "lidar_camera" );
	ros::NodeHandle nh;
	ros::Subscriber sub = nh.subscribe<sensor_msgs::LaserScan>("/scan", 1, scanCallback);
	ros::Subscriber sub2 = nh.subscribe("/distance",1,DistanceCallback);
	Mat img_frame;
	Mat img_show;
	LidarImage lidarImage;
	geometry_msgs::Twist msg_twist;
	ConeInfo pathResult;
	ConeDetect coneDetect;
	Mat onlineMap;
	double minfrequency=50,frequency;

	cv::VideoCapture capture( 0 );
	capture.set(CAP_PROP_FOURCC, cv::VideoWriter::fourcc('M','P','4','2'));
	// capture.set(CAP_PROP_AUTO_EXPOSURE,0.25);
	// capture.set(CAP_PROP_EXPOSURE,0.025); //29
	capture.set(CAP_PROP_FRAME_WIDTH,320);
	capture.set(CAP_PROP_FRAME_HEIGHT,240);
	// image_transport::ImageTransport it(nh);
	// image_transport::Publisher pub_image = it.advertise( "/camera", 1 );
	// image_transport::Publisher pub_onlineMap = it.advertise( "/online_map", 1 );
	ros::Publisher pub_vel = nh.advertise<geometry_msgs::Twist>("/cmd_vel", 1);
	if( not capture.isOpened() )
	{
		ROS_ERROR_STREAM("Failed to open camera !");
		ros::shutdown();
	}
	// clock_t start, laststart;
	double start2, laststart2;
	ros::spinOnce();
	while( ros::ok() )
	{
		// start = clock();
		start2 = ros::Time::now().toSec();
		capture >> img_frame;
		// if( img_frame.empty() )
		// {
		//     ROS_ERROR_STREAM( "Failed to capture frame!" );
		//     ros::shutdown();
		// }
		// resize(img_frame, img_frame, Size(320, 240), INTER_LINEAR);
		pathResult = lidarImage.draw(img_frame,scan_data);  //图像和激光雷达处理主函数
		ROS_INFO("endSignal%d",endSignal);
		if(endSignal == 1)
        {
        	msg_twist.angular.z = pathResult.angle;
            msg_twist.linear.x = 0;
        }
		else if(endSignal == 2)
        {
        	msg_twist.angular.z = pathResult.angle;
            msg_twist.linear.x = 1000;
        }
		else if(endSignal == 10)
        {
        	msg_twist.angular.z = pathResult.angle;
            msg_twist.linear.x = 2000;
        }
        else{
        	msg_twist.angular.z = pathResult.angle;
        	msg_twist.linear.x = pathResult.speed;
        }
		// onlineMap = pathResult.img;
		// sensor_msgs::ImagePtr msg_frame = cv_bridge::CvImage(std_msgs::Header(), "bgr8", img_frame).toImageMsg();
		// sensor_msgs::ImagePtr msg_Map = cv_bridge::CvImage(std_msgs::Header(), "bgr8", onlineMap).toImageMsg();
		// pub_image.publish(msg_frame);
		// pub_onlineMap.publish(msg_Map);
		// ROS_INFO("pathResult.angle:%f",pathResult.angle);
		pub_vel.publish(msg_twist); //发布线速度和角速度
		// imshow("onlineMap",onlineMap);
		imshow("video",img_frame);
		cv::waitKey( 1 );
		ros::spinOnce(); //更新激光雷达
		// ROS_INFO("frequency:%f", CLOCKS_PER_SEC / float((start - laststart)));
		frequency = 1 / (start2 - laststart2);
		ROS_INFO("frequency:%f", frequency);
		if(frequency < minfrequency && frequency > 5)
		{
			minfrequency = frequency;
			//ROS_INFO("minfrequency:%f", minfrequency);
		}
		// laststart = start;
		laststart2 = start2;
    }

  	capture.release(); 
  	return EXIT_SUCCESS;
}